#include "MapDrawer.h"
#include "KeyPoseFrame.h"
#include <pangolin/pangolin.h>
#include <mutex>

namespace msf
{

MapDrawer::MapDrawer(Atlas* pAtlas, Settings* settings):mpAtlas(pAtlas)
{
      if(settings)
      {
            newParameterLoader(settings);

            // Problem8 可视化坐标不对应
            // R = Rx(0)Ry(90)Rz(-90)
            Eigen::Matrix4f rotate_y;
            rotate_y << cos(M_PI / 2), 0, sin(M_PI / 2), 0,
                        0, 1, 0, 0,
                        -sin(M_PI / 2), 0, cos(M_PI / 2), 0,
                        0, 0, 0, 1;
            Eigen::Matrix4f rotate_z;
            rotate_z << cos(-M_PI / 2), -sin(-M_PI / 2), 0, 0,
                        sin(-M_PI / 2), cos(-M_PI / 2), 0, 0,
                        0, 0, 1, 0,
                        0, 0, 0, 1;
            rotate = rotate_y*rotate_z;            
      }
      else
      {
            std::cerr << "** Error in the config file! **"<< std::endl;
            try
            {
                  throw -1;
            }
            catch(exception &e)
            {

            }
      }
}

void MapDrawer::newParameterLoader(Settings *settings) {
    mKeyPoseFrameSize = settings->keyPoseFrameSize();
    mKeyPoseFrameLineWidth = settings->keyPoseFrameLineWidth();
    mGraphLineWidth = settings->graphLineWidth();
    mPointSize = settings->pointSize();
    mOdomSize = settings->odomSize();
    mOdomLineWidth  = settings->odomLineWidth();
}

void MapDrawer::DrawKeyPoseFrames(const bool bDrawKPF, const bool bDrawInertialGraph)
{
      const float &w = mKeyPoseFrameSize;
      const float h = w*0.75;
      const float z = w*0.6;

      Map* pActiveMap = mpAtlas->GetCurrentMap();
      // DEBUG LBA
      std::set<long unsigned int> sOptKPFs = pActiveMap->msOptKPFs;
      std::set<long unsigned int> sFixedKPFs = pActiveMap->msFixedKPFs;

      if(!pActiveMap)
            return;

      const vector<KeyPoseFrame*> vpKPFs = pActiveMap->GetAllKeyPoseFrames();

      if(bDrawKPF)
      {
            for(size_t i=0; i<vpKPFs.size(); i++)
            {
                  KeyPoseFrame* pKPF = vpKPFs[i];
                  Eigen::Matrix4f Two = pKPF->GetPoseInverse().matrix();

                  // Problem8.1 可视化坐标不对应
                  // 将Two根据rotate进行旋转
                  //Two.block<3,3>(0,0) = rotate.block<3,3>(0,0) * Two.block<3,3>(0,0);
                  Two.block<3,3>(0,0) =   Two.block<3,3>(0,0) * rotate.block<3,3>(0,0);
                  unsigned int index_color = pKPF->mnOriginMapId;

                  glPushMatrix();

                  glMultMatrixf((GLfloat*)Two.data());

                  if(!pKPF->GetParent()) // It is the first KF in the map
                  {
                        glLineWidth(mKeyPoseFrameLineWidth*5);
                        glColor3f(1.0f,0.0f,0.0f); //red
                        glBegin(GL_LINES);
                  }
                  else
                  {
                        glLineWidth(mKeyPoseFrameLineWidth);
                        //if (bDrawOptLba) { //??
                        if (0) {      
                              if(sOptKPFs.find(pKPF->mnId) != sOptKPFs.end())
                              {
                                    glColor3f(0.0f,1.0f,0.0f); // Green -> Opt KFs
                              }
                              else if(sFixedKPFs.find(pKPF->mnId) != sFixedKPFs.end())
                              {
                                    glColor3f(1.0f,0.0f,0.0f); // Red -> Fixed KFs
                              }
                              else
                              {
                                    glColor3f(0.0f,0.0f,1.0f); // Basic color
                              }
                        }
                        else
                        {
                              glColor3f(0.0f,0.0f,1.0f); // Basic color
                        }
                        glBegin(GL_LINES);
                  }

                  glVertex3f(0,0,0);
                  glVertex3f(w,h,z);
                  glVertex3f(0,0,0);
                  glVertex3f(w,-h,z);
                  glVertex3f(0,0,0);
                  glVertex3f(-w,-h,z);
                  glVertex3f(0,0,0);
                  glVertex3f(-w,h,z);

                  glVertex3f(w,h,z);
                  glVertex3f(w,-h,z);

                  glVertex3f(-w,h,z);
                  glVertex3f(-w,-h,z);

                  glVertex3f(-w,h,z);
                  glVertex3f(w,h,z);

                  glVertex3f(-w,-h,z);
                  glVertex3f(w,-h,z);
                  glEnd();

                  glPopMatrix();

                  glEnd();
            }
      }

      if(bDrawInertialGraph && pActiveMap->isImuInitialized())
      {
            glLineWidth(mGraphLineWidth);
            glColor4f(1.0f,0.0f,0.0f,0.6f);
            glBegin(GL_LINES);

            //Draw inertial links
            for(size_t i=0; i<vpKPFs.size(); i++)
            {
                  KeyPoseFrame* pKPFi = vpKPFs[i];
                  Eigen::Vector3f Ow = pKPFi->GetOdomCenter();
                  // Problem8.2 可视化坐标不对应
                  //Ow = rotate.block<3,3>(0,0) * Ow;
                  KeyPoseFrame* pNext = pKPFi->mNextKPF;
                  if(pNext)
                  {
                        Eigen::Vector3f Owp = pNext->GetOdomCenter();
                        // Problem8.3 可视化坐标不对应
                        //Owp = rotate.block<3,3>(0,0) * Owp;
                        glVertex3f(Ow(0),Ow(1),Ow(2));
                        glVertex3f(Owp(0),Owp(1),Owp(2));
                  }
            }

            glEnd();
      }

      vector<Map*> vpMaps = mpAtlas->GetAllMaps();

      if(bDrawKPF)
      {
            for(Map* pMap : vpMaps)
            {
                  if(pMap == pActiveMap)
                        continue;
                  
                  vector<KeyPoseFrame*> vpKPFs = pMap->GetAllKeyPoseFrames();

                  for(size_t i=0; i<vpKPFs.size(); i++)
                  {
                        KeyPoseFrame* pKPF = vpKPFs[i];
                        Eigen::Matrix4f Two = pKPF->GetPoseInverse().matrix();
                        // Problem8.4 可视化坐标不对应
                        //Two.block<3,3>(0,0) = rotate.block<3,3>(0,0) * Two.block<3,3>(0,0);
                        Two.block<3,3>(0,0) = Two.block<3,3>(0,0) * rotate.block<3,3>(0,0);
                        unsigned int index_color = pKPF->mnOriginMapId;

                        glPushMatrix();

                        glMultMatrixf((GLfloat*)Two.data());

                        if(!vpKPFs[i]->GetParent()) // It is the first KPF in the map
                        {
                              glLineWidth(mKeyPoseFrameLineWidth*5);
                              glColor3f(1.0f,0.0f,0.0f);
                              glBegin(GL_LINES);
                        }
                        else
                        {
                              glLineWidth(mKeyPoseFrameLineWidth);
                              glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]);
                              glBegin(GL_LINES);
                        }

                        glVertex3f(0,0,0);
                        glVertex3f(w,h,z);
                        glVertex3f(0,0,0);
                        glVertex3f(w,-h,z);
                        glVertex3f(0,0,0);
                        glVertex3f(-w,-h,z);
                        glVertex3f(0,0,0);
                        glVertex3f(-w,h,z);

                        glVertex3f(w,h,z);
                        glVertex3f(w,-h,z);

                        glVertex3f(-w,h,z);
                        glVertex3f(-w,-h,z);

                        glVertex3f(-w,h,z);
                        glVertex3f(w,h,z);

                        glVertex3f(-w,-h,z);
                        glVertex3f(w,-h,z);
                        glEnd();

                        glPopMatrix();
                  }
            }
      }

}

// 绘制Odom位姿在OpenGL窗口中的图像
void MapDrawer::DrawCurrentOdom(pangolin::OpenGlMatrix &Two)
{
      // 定义坐标系箭头的大小和高度
      const float &w = mOdomSize;
      const float h = w*0.75;
      const float z = w*0.6;

      // 保存当前的OpenGL矩阵，以便后续的绘制
      glPushMatrix();

      // 用传入的变换矩阵对OpenGL矩阵进行变换
#ifdef HAVE_GLES
      glMultMatrixf(Two.m);
#else
      glMultMatrixd(Two.m);
#endif

      // 设置线宽和颜色
      glLineWidth(mOdomLineWidth);
      glColor3f(0.0f,1.0f,0.0f);

      // 开始绘制坐标系箭头，使用GL_LINES模式
      glBegin(GL_LINES);
      // 绘制箭头末端四个点和坐标系原点的连线
      glVertex3f(0,0,0);
      glVertex3f(w,h,z);
      glVertex3f(0,0,0);
      glVertex3f(w,-h,z);
      glVertex3f(0,0,0);
      glVertex3f(-w,-h,z);
      glVertex3f(0,0,0);
      glVertex3f(-w,h,z);

      // 绘制箭头的四个侧面
      glVertex3f(w,h,z);
      glVertex3f(w,-h,z);

      glVertex3f(-w,h,z);
      glVertex3f(-w,-h,z);

      glVertex3f(-w,h,z);
      glVertex3f(w,h,z);

      glVertex3f(-w,-h,z);
      glVertex3f(w,-h,z);
      glEnd();
      
      // 恢复OpenGL矩阵
      glPopMatrix();
}

void MapDrawer::SetCurrentOdomPose(const Sophus::SE3f &Tow)
{
    unique_lock<mutex> lock(mMutexOdom);
    mOdomPose = Tow.inverse();
}

void MapDrawer::GetCurrentOpenGLOdomMatrix(pangolin::OpenGlMatrix &M, pangolin::OpenGlMatrix &MOw)
{
      Eigen::Matrix4f Two;
      {
            unique_lock<mutex> lock(mMutexOdom);
            Two = mOdomPose.matrix();
            // Problem8.5 可视化坐标不对应
            Two.block<3,3>(0,0) = Two.block<3,3>(0,0) * rotate.block<3,3>(0,0);
      }

      for (int i = 0; i<4; i++) {
            M.m[4*i] = Two(0,i);
            M.m[4*i+1] = Two(1,i);
            M.m[4*i+2] = Two(2,i);
            M.m[4*i+3] = Two(3,i);
      }

      MOw.SetIdentity();
      MOw.m[12] = Two(0,3);
      MOw.m[13] = Two(1,3);
      MOw.m[14] = Two(2,3);
}

} //namespace msf